#ifndef AXINPUTOUTPUTHANDLER_H
#define AXINPUTOUTPUTHANDLER_H

#include <QObject>
#include "Aria.h"
#include "ArNetworking.h"
#include "ArClientRatioDrive.h"
#define VEL_AMOUNT 100
#define ROT_AMOUNT 20

static double tempX=0,tempY=0,tempA=0,tempB=0,tempV=0,mousePoseX=0,mousePoseY=0,mousePoseTh=0,localScore=0;
static double tempVV = 0;
static double deltaSpeed=0;

class InputHandler
{
public: 
  /** 
   * @param client  Our client networking object 
   * @param keyHandler  Key handler to register command callbacks with
   */
  InputHandler(ArClientBase *client);
  InputHandler(ArClientBase *client, ArKeyHandler *keyHandler);
  virtual ~InputHandler(void);

  /// Up arrow key handler: drive the robot forward
  void up(void);

  /// Down arrow key handler: drive the robot backward
  void down(void);

  /// Left arrow key handler: turn the robot left 
  void left(void);

  /// Right arrow key handler: turn the robot right
  void right(void);

  /// Move the robot laterally right  (q key)
  void lateralLeft(void);

  /// Move the robot laterally right  (e key)
  void lateralRight(void);

  /// Send drive request to the server with stored values
  void sendInput(void);

public:
  /// Send a request to enable "safe drive" mode on the server
  void safeDrive();

  /// Send a request to disable "safe drive" mode on the server
  void unsafeDrive();

  /// Request stop
  void space();

  void listData();

  void logTrackingTerse();
  void logTrackingVerbose();
  void resetTracking();
  void toggleDebug();

  ArClientBase *myClient;
  ArKeyHandler *myKeyHandler;

  /// Set this to true in the constructor to print out debugging information
  bool myPrinting;

  /// Object that continuously sends driving requests in the background. 
  ArClientRatioDrive myDriveClient;

  ArFunctorC<InputHandler> myUpCB;
  ArFunctorC<InputHandler> myDownCB;
  ArFunctorC<InputHandler> myLeftCB;
  ArFunctorC<InputHandler> myRightCB;
  ArFunctorC<InputHandler> myLateralLeftCB;
  ArFunctorC<InputHandler> myLateralRightCB;
  ArFunctorC<InputHandler> mySafeDriveCB;
  ArFunctorC<InputHandler> myUnsafeDriveCB;
  ArFunctorC<InputHandler> myListDataCB;
  ArFunctorC<InputHandler> myLogTrackingTerseCB;
  ArFunctorC<InputHandler> myLogTrackingVerboseCB;
  ArFunctorC<InputHandler> myResetTrackingCB;
  ArFunctorC<InputHandler> mySpaceCB;
  ArFunctorC<InputHandler> myToggleDebugCB;
};

class OutputHandler
{
public:
  OutputHandler(ArClientBase *client);
  virtual ~OutputHandler(void);
  
  /// This callback is called when an update on general robot state arrives
  void handleOutput(ArNetPacket *packet);
  /// This callback is called when an update on general robot state arrives
  void handleOutputNumbers(ArNetPacket *packet);
  /// This callback is called when an update on general robot state arrives
  void handleOutputStrings(ArNetPacket *packet);

  /// This callback is called when an update on the battery configuration changes
  void handleBatteryInfo(ArNetPacket *packet);
  /// This is called when the physical robot information comes back
  void handlePhysicalInfo(ArNetPacket *packet);
  /// This callback is called when an update on the temperature information changes
  void handleTemperatureInfo(ArNetPacket *packet);
  /// Called when the map on the server changes.
  void handleMapUpdated(ArNetPacket *packet);
protected:

  /// The results from the data update are stored in these variables
  //@{
  double myX;
  double myY;
  double myTh;
  double myVel;
  double myRotVel;
  double myLatVel;
  bool myVoltageIsStateOfCharge;
  char myTemperature;
  double myVoltage;
  char myStatus[256];
  char myMode[256];
  //@}
  ArClientBase *myClient;

  /** These functor objects are given to the client to receive updates when they
   * arrive from the server.
   */
  //@{
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputNumbersCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleOutputStringsCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleBatteryInfoCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandlePhysicalInfoCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleTemperatureInfoCB;
  ArFunctor1C<OutputHandler, ArNetPacket *> myHandleMapUpdatedCB;
  //@}
  
  /// A header for the columns in the data printout is sometimes printed
  bool myNeedToPrintHeader;
  /// Don't print any information until we get the battery info
  bool myGotBatteryInfo;
};

#endif // AXINPUTOUTPUTHANDLER_H
